#include "PointCloudWidget.h"
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <iostream>

PointCloudWidget::PointCloudWidget(QWidget* parent)
    : QOpenGLWidget(parent), m_cloud(new pcl::PointCloud<pcl::PointXYZ>)
{
    loadPointCloud();
    m_octree = new Octree(0.001,50);
    m_octree->buildTree(m_cloud);
    std::cout << m_octree->getMaxDepth() << std::endl;
}

PointCloudWidget::~PointCloudWidget()
{
    makeCurrent();
    for (auto vbo : m_vbos) {
        glDeleteBuffers(1, &vbo);
    }
    glDeleteProgram(m_shaderProgram);
    doneCurrent();
}

// 加载点云
void PointCloudWidget::loadPointCloud()
{
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\Work\\Work\\Codes\\2025\\point_cloud.pcd", *m_cloud) == -1) {
        std::cerr << "Failed to load point cloud." << std::endl;
    }

    std::cout << "Loaded point cloud with " << m_cloud->size() << " points." << std::endl;

    // 计算点云的中心
    pcl::PointXYZ m_cloudCenter ;
    if (!m_cloud->empty()) {
        pcl::getMinMax3D(*m_cloud, minPt, maxPt);
        m_cloudCenter = pcl::PointXYZ((minPt.x + maxPt.x) * 0.5f,
                                      (minPt.y + maxPt.y) * 0.5f,
                                      (minPt.z + maxPt.z) * 0.5f);
    } else {
        m_cloudCenter = pcl::PointXYZ(0.0f, 0.0f, 0.0f);  // 如果点云为空，默认中心点为原点
    }
    glm::vec3 center = glm::vec3(m_cloudCenter.x,m_cloudCenter.y,m_cloudCenter.z);

    // 计算初始相机位置
    float boundingBoxSize = glm::length(center); // 近似包围盒大小
    m_cameraPos = center + glm::vec3(0.0f, 0.0f, boundingBoxSize * 1.5f);
    m_cameraFront = glm::normalize(center - m_cameraPos);  // 指向点云中心
    m_cameraUp = glm::vec3(0.0f, 1.0f, 0.0f);
}

// OpenGL 初始化
void PointCloudWidget::initializeGL()
{
    initializeOpenGLFunctions();
    glEnable(GL_DEPTH_TEST);
    glPointSize(1.0f);

    // 调试：输出 OpenGL 信息
    const GLubyte* version = glGetString(GL_VERSION);
    std::cout << "OpenGL Version: " << version << std::endl;

    // 初始化着色器
    const char* vsSource = R"(
            #version 330 core
            layout(location = 0) in vec3 position;
            uniform mat4 projection;
            uniform mat4 view;
            uniform mat4 model;
            uniform int colorComponent; // 用户设定的颜色分量：0 for x, 1 for y, 2 for z
            uniform float minValue;     // 最小值
            uniform float maxValue;     // 最大值
            out vec3 vertexColor;       // 传递给片段着色器的颜色

            void main() {
                gl_Position = projection * view * model * vec4(position, 1.0);

                // 根据用户设定的颜色分量获取坐标值
                float value = 0.0;
                if (colorComponent == 0) {
                    value = position.x;
                } else if (colorComponent == 1) {
                    value = position.y;
                } else if (colorComponent == 2) {
                    value = position.z;
                }

                // 归一化坐标值到 [0, 1] 范围
                value = clamp((value - minValue) / (maxValue - minValue), 0.0, 1.0);

                // 计算从蓝色到红色的颜色
                float r = value;
                float g = 1.0 - abs(value - 0.5) * 2.0;
                float b = 1.0 - value;

                // 组合 RGB 颜色
                vertexColor = vec3(r, g, b);
            }
        )";

    const char* fsSource = R"(
            #version 330 core
            in vec3 vertexColor; // 接收从顶点着色器传递过来的颜色
            out vec4 fragColor;

            void main() {
                fragColor = vec4(vertexColor, 1.0); // 设置片元颜色
            }
        )";

    GLuint vs = glCreateShader(GL_VERTEX_SHADER);
    glShaderSource(vs, 1, &vsSource, nullptr);
    glCompileShader(vs);

    GLint success;
    GLchar infoLog[512];
    glGetShaderiv(vs, GL_COMPILE_STATUS, &success);
    if (!success) {
        glGetShaderInfoLog(vs, 512, nullptr, infoLog);
        std::cerr << "ERROR::SHADER::VERTEX::COMPILATION_FAILED\n" << infoLog << std::endl;
    }

    GLuint fs = glCreateShader(GL_FRAGMENT_SHADER);
    glShaderSource(fs, 1, &fsSource, nullptr);
    glCompileShader(fs);

    glGetShaderiv(fs, GL_COMPILE_STATUS, &success);
    if (!success) {
        glGetShaderInfoLog(fs, 512, nullptr, infoLog);
        std::cerr << "ERROR::SHADER::FRAGMENT::COMPILATION_FAILED\n" << infoLog << std::endl;
    }

    m_shaderProgram = glCreateProgram();
    glAttachShader(m_shaderProgram, vs);
    glAttachShader(m_shaderProgram, fs);
    glLinkProgram(m_shaderProgram);
    glDeleteShader(vs);
    glDeleteShader(fs);

    // 检查 Program 链接是否成功
    glGetProgramiv(m_shaderProgram, GL_LINK_STATUS, &success);
    if (!success) {
        glGetProgramInfoLog(m_shaderProgram, 512, nullptr, infoLog);
        std::cerr << "ERROR::SHADER::PROGRAM::LINKING_FAILED\n" << infoLog << std::endl;
    }

    // 调试：确认着色器已加载
    std::cout << "Shader program successfully loaded." << std::endl;
}

// 渲染函数
void PointCloudWidget::paintGL()
{
    glClearColor(0.1f, 0.1f, 0.1f, 1.0f);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // 在渲染之前输出 OpenGL 错误状态
    GLenum err;
    while ((err = glGetError()) != GL_NO_ERROR) {
        std::cerr << "OpenGL Error before rendering: " << err << std::endl;
    }

    if (!m_cloud) return;

    // 调试：检查 OpenGL 状态
    std::cout << "Rendering point cloud..." << std::endl;

    glm::mat4 projection = glm::perspective(glm::radians(45.0f),
                                            static_cast<float>(width()) / height(), 0.01f, 1000.0f);
    glm::mat4 view = glm::lookAt(m_cameraPos + m_panOffset,
                                 m_cameraPos + m_panOffset + m_cameraFront,
                                 m_cameraUp);
    glm::mat4 model = glm::mat4(1.0f);

    // 传递矩阵到着色器
    glUseProgram(m_shaderProgram);
    glUniformMatrix4fv(glGetUniformLocation(m_shaderProgram, "projection"),
                       1, GL_FALSE, glm::value_ptr(projection));
    glUniformMatrix4fv(glGetUniformLocation(m_shaderProgram, "view"),
                       1, GL_FALSE, glm::value_ptr(view));
    glUniformMatrix4fv(glGetUniformLocation(m_shaderProgram, "model"),
                       1, GL_FALSE, glm::value_ptr(model));

    // 设置颜色分量（例如，使用 z 坐标）
    GLint colorComponentLoc = glGetUniformLocation(m_shaderProgram, "colorComponent");
    GLint minValueLoc = glGetUniformLocation(m_shaderProgram, "minValue");
    GLint maxValueLoc = glGetUniformLocation(m_shaderProgram, "maxValue");
    int colorComponent = 1; // 0 for x, 1 for y, 2 for z
    // 设置最小值和最大值
    float minValue = 0.0f;
    float maxValue = 0.0f;
    if (colorComponent == 0) {
        minValue = minPt.x;
        maxValue = maxPt.x;
    } else if (colorComponent == 1) {
        minValue = minPt.y;
        maxValue = maxPt.y;
    } else if (colorComponent == 2) {
        minValue = minPt.z;
        maxValue = maxPt.z;
    }
    glUniform1f(minValueLoc, minValue);
    glUniform1f(maxValueLoc, maxValue);
    glUniform1i(colorComponentLoc, colorComponent);

    // 调试：输出绑定的 VBO 状态
    std::cout << "Binding VBO..." << std::endl;
#if 1
    GLuint vbo;
    uploadToVBO(m_cloud, vbo);
    glBindBuffer(GL_ARRAY_BUFFER, vbo);
    glEnableVertexAttribArray(0);
    glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, sizeof(pcl::PointXYZ), nullptr);

    // 绘制
    glDrawArrays(GL_POINTS, 0, m_cloud->points.size());

    // 删除 VBO
    glDeleteBuffers(1, &vbo);

#else
    // 遍历八叉树节点，进行 LOD 和视锥剔除
    for (auto const& node = m_octree->leaf_depth_begin(); node != m_octree->leaf_depth_end(); ++node) {
        glm::vec3 nodeCenter = *node;
        float nodeSize = node.getSize();

        // 进行视锥剔除
        if (!isNodeInFrustum(nodeCenter, nodeSize, frustum)) continue;

        // 根据与相机的距离和节点的大小选择 LOD 层次
        int lodLevel = selectLODLevel(nodeCenter, nodeSize, 0);

        // 如果节点在当前 LOD 层次内，则渲染
        if (lodLevel <= m_currentLODLevel) {
            // 渲染节点（此部分取决于你具体的渲染方法）
            GLuint vbo;
            uploadToVBO(node.getPointCloud(), vbo);
            glBindBuffer(GL_ARRAY_BUFFER, vbo);
            glEnableVertexAttribArray(0);
            glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, sizeof(pcl::PointXYZ), nullptr);

            // 绘制点云或节点的简化表示
            glDrawArrays(GL_POINTS, 0, node.getPointCloud()->size());

            // 绘制后删除 VBO
            glDeleteBuffers(1, &vbo);
        }
    }
#endif

    // 调试：检查 OpenGL 状态
    err = glGetError();
    if (err != GL_NO_ERROR) {
        std::cerr << "OpenGL Error during rendering: " << err << std::endl;
    }
}

// 上传点云到 GPU
void PointCloudWidget::uploadToVBO(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, GLuint& vbo)
{
    glGenBuffers(1, &vbo);
    glBindBuffer(GL_ARRAY_BUFFER, vbo);

    glBufferData(GL_ARRAY_BUFFER, cloud->size() * sizeof(pcl::PointXYZ), 
                 cloud->points.data(), GL_STATIC_DRAW);
    // 调试：检查 VBO 是否上传成功
    GLenum err = glGetError();
    if (err != GL_NO_ERROR) {
        std::cerr << "ERROR::VBO::UPLOAD_FAILED, Error Code: " << err << std::endl;
    } else {
        std::cout << "VBO uploaded successfully." << std::endl;
    }
}

void PointCloudWidget::resizeGL(int width, int height)
{
    glViewport(0, 0, width, height);
    m_projection = glm::perspective(glm::radians(45.0f),
                                    static_cast<float>(width) / height, 0.1f, 1000.0f);
}

// 选择 LOD 层次（根据距离和节点大小）
int PointCloudWidget::selectLODLevel(const glm::vec3& nodeCenter,
                                     float nodeSize,
                                     int currentDepth)
{
    float distance = glm::length(m_cameraPos - nodeCenter);
    float screenSpaceError = nodeSize / distance; // 简化的屏幕空间误差
    return (screenSpaceError > 0.1f) ? currentDepth + 1 : currentDepth;
}

// 提取视锥体平面
PointCloudWidget::Frustum PointCloudWidget::extractFrustum(const glm::mat4& viewProj)
{
    Frustum frustum;
    const glm::mat4 m = glm::transpose(viewProj); // PCL使用行优先，需转置

    // 提取六个平面方程
    frustum.planes[0] = m[3] + m[0]; // 左
    frustum.planes[1] = m[3] - m[0]; // 右
    frustum.planes[2] = m[3] + m[1]; // 下
    frustum.planes[3] = m[3] - m[1]; // 上
    frustum.planes[4] = m[3] + m[2]; // 近
    frustum.planes[5] = m[3] - m[2]; // 远

    // 归一化平面方程
    for (auto& plane : frustum.planes) {
        float length = glm::length(glm::vec3(plane));
        plane /= length;
    }
    return frustum;
}

// 判断节点是否在视锥体内
bool PointCloudWidget::isNodeInFrustum(const glm::vec3& center,
                                       float size,
                                       const Frustum& frustum)
{
    const glm::vec3 halfSize = glm::vec3(size / 2.0f);
    const glm::vec3 min = center - halfSize;
    const glm::vec3 max = center + halfSize;

    for (const auto& plane : frustum.planes) {
        const glm::vec3 normal = glm::vec3(plane);
        const float d = plane.w;

        // 计算包围盒顶点在平面上的投影
        float projMin = glm::dot(min, normal) + d;
        float projMax = glm::dot(max, normal) + d;

        // 如果所有顶点在平面负半空间，则不可见
        if (projMin < 0 && projMax < 0) return false;
    }
    return true;
}

// 鼠标交互实现
void PointCloudWidget::mousePressEvent(QMouseEvent* event)
{
    m_lastMousePos = event->pos();
}

void PointCloudWidget::mouseMoveEvent(QMouseEvent* event)
{
    float dx = event->x() - m_lastMousePos.x();
    float dy = event->y() - m_lastMousePos.y();

    if (event->buttons() & Qt::LeftButton) {
        // 旋转控制
        m_yaw += dx * 0.1f;
        m_pitch += dy * 0.1f;
        m_pitch = glm::clamp(m_pitch, -89.0f, 89.0f);

        glm::vec3 front;
        front.x = cos(glm::radians(m_yaw)) * cos(glm::radians(m_pitch));
        front.y = sin(glm::radians(m_pitch));
        front.z = sin(glm::radians(m_yaw)) * cos(glm::radians(m_pitch));
        m_cameraFront = glm::normalize(front);
    }

    if (event->buttons() & Qt::RightButton) {
        QPoint delta = event->pos() - m_lastMousePos;  // 计算鼠标偏移量
        m_lastMousePos = event->pos();  // 更新鼠标位置

        float moveSpeed = 0.05f;  // 控制平移速度

        // 计算相机的右方向和上方向
        glm::vec3 right = glm::normalize(glm::cross(m_cameraFront, m_cameraUp));
        glm::vec3 up = glm::normalize(m_cameraUp);

        // 计算平移量
        m_panOffset -= right * (float)delta.x() * moveSpeed;
        m_panOffset += up * (float)delta.y() * moveSpeed;
    }

    m_lastMousePos = event->pos();
    update();
}

void PointCloudWidget::wheelEvent(QWheelEvent* event)
{
    m_cameraPos -= m_cameraFront * static_cast<float>(event->angleDelta().y()) * m_zoomSpeed;
    update();
}
